/*
	Copyright (c) 2010 <copyright holders>
	This source code is licensed under The MIT License,
	see the file LICENSE for details.
	*/

#include "Squad.hpp"

#include "UnitInvariant.hpp"
#include "World.hpp"

namespace spelprog2 {
namespace sim {

Squad::Pointer Squad::create(Int playerId, Int id, World & world, const Int2d & position,
    Int numUnits, const std::string & squadType)
{
    Squad::Pointer squad(new Squad(playerId, id, world, position, numUnits, squadType));

    squad->orders_.push_back(SquadOrder::Pointer(new IdleSquadOrder(*squad.get())));

    //making unit initialization here as it will request shared pointer
    for (Int i = 0; i < numUnits; i += 1) {
        //TODO: take squad type into consideration
        squad->units_.push_back(Unit::create(UnitInvariants::getInstance().find(squadType), playerId, *squad.get(), squad->world_,
            position + squad->calcUnitPosition(i, numUnits), squad->calcUnitFacing(i, numUnits)));
    }

    return squad;
}

Squad::Squad(Int playerId, Int id, World & world, const Int2d & position,
        Int numUnits, const std::string & squadType) : boost::enable_shared_from_this<Squad>(),
    id_(id),
    playerId_(playerId),
    world_(world),
    units_(),
    formation_(auxmsg::WEDGE),
    typicalRadius_(2600), //TODO: load from squad type
    formationWidth_(numUnits.sqrt() + Int(1)),
    facing_(0), //TODO: do something about this
    orders_()
{
}

Int2d Squad::calcUnitPosition(Int index, Int n) const
{
    //1) Calculate position in squad space for facing 0 (looking towards +x axis)

    Int2d pos;
    //line formations
    if (formation_ == auxmsg::DENSE_LINE || formation_ == auxmsg::LOOSE_LINE) {
        const Int radius = typicalRadius_ * (formation_ == auxmsg::LOOSE_LINE? (Int(2)) : (Int(1)));
        pos.x = -radius * (index - n / Int(2)) / formationWidth_;
        pos.y = +radius * ((index % formationWidth_) -  formationWidth_ / Int(2));
    }
    else if (formation_ == auxmsg::WEDGE) {
        const Int col = Int(int(std::sqrt(float(index)))); //special sqrt prevents rounding
        const Int row = index - col * col;
        const Int colOffset = Int(int(std::sqrt(float(n)))) / Int(2); //basically half the number of columns
        const Int rowOffset = col;
        pos.x = (colOffset - col) * typicalRadius_;
        pos.y = (row - rowOffset) * typicalRadius_;
    }

    //2) Rotate around center by face angle ===================================

    float cos = std::cos(FLOAT_PI * float(facing_) / float(sim::ANGULAR_UNITS_PER_HALFCIRCLE));
    float sin = std::sin(FLOAT_PI * float(facing_) / float(sim::ANGULAR_UNITS_PER_HALFCIRCLE));
    
    return Int2d(float(pos.x) * cos - float(pos.y) * sin,
        float(pos.y) * cos + float(pos.x) * sin);
}

Int Squad::calcUnitFacing(Int index, Int n) const
{
    Int angle = facing_;
    //TODO: modify for formations where not everyone face same direction (box?)
    return angle;
}

void Squad::simUpdate(Int sequenceNbr)
{
    if (orders_.size() > 1 && orders_.front()->finished()) {
        orders_.pop_front();
    }

    SquadOrder::Pointer order = orders_.front();
    order->simUpdate(sequenceNbr);

    //update units of this squad
    for (Units::iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        (*it)->simUpdate(sequenceNbr);
    }
}

void Squad::setFacing(Int newFacing)
{
    assert(newFacing.abs() <= ANGULAR_UNITS_PER_HALFCIRCLE);
    facing_ = newFacing;
}

void Squad::setTarget(const Int2d & position)
{
    Int index(0);
    const Int nReady = countReadyUnits();

	for (Units::iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        if (!(*it)->readyForNewTarget()) {
            continue;
        }
        
        const Int2d unitPosition = position + calcUnitPosition(index, nReady);
        const Int unitFacing = calcUnitFacing(index, nReady);
		(*it)->setTarget(unitPosition, unitFacing);
        index += 1;
	}
}

void Squad::setTeam(Int team)
{
    for (Units::iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
		(*it)->setTeam(team);
	}
}

void Squad::retargetSquad(Squad & other)
{
    //O(n * m), see if this can be done better later on (I have some ideas but they require randomness)
    for (Units::iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        Unit::Pointer myUnit = *it;

        if (!myUnit->readyForNewTarget()) {
            continue;
        }

        WideInt bestDistSqr(WIDE_INT_MAX);
        Unit::Pointer bestTarget;
        for (Units::iterator jt = other.units_.begin(), end2 = other.units_.end(); jt != end2; ++jt) {
            Unit::Pointer target = *jt;

            if (target->isDead() || target->isFleeing()) {
                continue;
            }

            const WideInt distSqr = (target->getPosition() - myUnit->getPosition()).squaredLength();
            if (distSqr < bestDistSqr) {
                bestDistSqr = distSqr;
                bestTarget = target;
            }
        }

        myUnit->setTarget(bestTarget);
    }


    //finally, reset orders
    queueOrder(SquadOrder::Pointer(new IdleSquadOrder(*this)), true);
}

void Squad::setFormation(auxmsg::Formation newFormation)
{
    formation_ = newFormation;
}

bool Squad::getOptimalPath(const Int2d &target, LandscapeSpatials::PointList &list)
{
    // TODO: Input squad specific parameters.
    world_.getLandscape().getSpatials().initPathFinder();
    world_.getLandscape().getSpatials().findGroundUnitPath(getPosition(), target, list);
    if(list.size() > 0) return true;
    return false;
}

Int2d Squad::getPosition() const
{
    assert(!units_.empty());

    WideInt sumX(0);
    WideInt sumY(0);
    Int2d anyPosition;

    for (Units::const_iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        Unit::ConstPointer unit = *it;
        anyPosition = unit->getPosition();

        if (unit->isActive()) {
            sumX += WideInt(unit->getPosition().x);
            sumY += WideInt(unit->getPosition().y);
        }
    }

    Int nActive = countActiveUnits();
    if (nActive == Int()) {
        return anyPosition;
    }
    
    return Int2d(sumX / WideInt(nActive), sumY / WideInt(nActive));
}

Int Squad::countAliveUnits() const
{
    Int alive(0);
    for (Units::const_iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        if (!(*it)->isDead()) {
            alive += 1;
        }
    }

    return alive;
}

Int Squad::countActiveUnits() const
{
    Int active(0);
    for (Units::const_iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        if ((*it)->isActive()) {
            active += 1;
        }
    }

    return active;
}

Int Squad::countReadyUnits() const
{
    Int ready(0);
    for (Units::const_iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        if ((*it)->readyForNewTarget()) {
            ready += 1;
        }
    }

    return ready;
}

bool Squad::allActiveUnitsIdle() const
{
    for (Units::const_iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        if ((*it)->isActive() && !(*it)->isIdle()) {
            return false;
        }
    }

    return true;
}

bool Squad::isDefeated() const
{
    for (Units::const_iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        if ((*it)->isActive()) {
            return false;
        }
    }

    return true;
}

Squad::Pointer Squad::beingAttackedBySquad()
{
    for (Units::iterator it = units_.begin(), end = units_.end(); it != end; ++it) {
        if ((*it)->isFighting()) {
            return (*it)->getTargetUnit()->getSquad().lock();
        }
    }

    return Squad::Pointer();
}

void Squad::queueOrder(SquadOrder::Pointer order, bool overwrite)
{
    if (overwrite) {
        orders_.clear();
    }

    orders_.push_back(order);
}

StopSquadOrder::StopSquadOrder(Squad & squad) : IdleSquadOrder(squad)
{
    squad_.setTarget(squad_.getPosition());
}

bool IdleSquadOrder::simUpdate(Int sequenceNbr)
{
    Squad::Pointer attacker = squad_.beingAttackedBySquad();
    if (attacker) {
        squad_.queueOrder(SquadOrder::Pointer(new ChargeSquadOrder(squad_, *attacker.get())), true);
        return false;
    }

    return true;
}

MoveSquadOrder::MoveSquadOrder(Squad & squad, const Int2d & target) : IdleSquadOrder(squad),
    target_(target),
    state_(FACE_DIRECTION)
{
    Int2d toTarget = target - squad_.getPosition();
    
    if(squad.getOptimalPath(target, path_))
    {
        toTarget = target - path_.back();
        target_ = path_.back();
    }

    if (toTarget != Int2d()) {
        const Int facing = toTarget.atan2();
        squad.setFacing(facing);
    }

    squad_.setTarget(target_); //makes us move to center of this formation but with updated facing
}

void MoveSquadOrder::toNextWaypoint()
{
    //TODO: finish this when pathfinding is done
    state_ = NEXT_WAYPOINT;
    target_ = path_.back();
    path_.pop_back();
    squad_.setTarget(target_);
}

bool MoveSquadOrder::simUpdate(Int sequenceNbr)
{
    if (!IdleSquadOrder::simUpdate(sequenceNbr)) {
        return false;
    }
    if (state_ == FACE_DIRECTION) {
        if (squad_.allActiveUnitsIdle()) { //every unit idle
            toNextWaypoint();
            return false;
        }
    }
    else if (state_ == NEXT_WAYPOINT && (squad_.getPosition() - target_).length() < squad_.getSize() && !path_.empty())
    {
        toNextWaypoint();
        return false;
    }

    return true;
}

bool MoveSquadOrder::finished()
{
    return state_ == FINISHED;
}

AttackSquadOrder::AttackSquadOrder(Squad & squad, Squad & target) :
    MoveSquadOrder(squad, target.getPosition()), target_(target.shared_from_this())
{
}

ChangeFormationSquadOrder::ChangeFormationSquadOrder(Squad & squad, auxmsg::Formation target) : IdleSquadOrder(squad),
    target_(target)
{
    squad_.setFormation(target);
    squad_.setTarget(squad_.getPosition());
}

bool ChangeFormationSquadOrder::simUpdate(Int sequenceNbr)
{
    if (!IdleSquadOrder::simUpdate(sequenceNbr)) {
        return false;
    }

    return true;
}

bool ChangeFormationSquadOrder::finished()
{
    return squad_.allActiveUnitsIdle();
}

ChargeSquadOrder::ChargeSquadOrder(Squad & squad, Squad & target) : IdleSquadOrder(squad),
    target_(target.shared_from_this()),
    finished_(false)
{
    squad_.retargetSquad(target);
}

bool ChargeSquadOrder::simUpdate(Int sequenceNbr)
{
    const Int RETARGET_PERIOD(3);

    if (finished_) {
        if (!IdleSquadOrder::simUpdate(sequenceNbr)) {
            return false;
        }
    }
    else {
        if (target_.expired()) {
            return false;
        }

        Squad::Pointer target = target_.lock();

        if (!target->isDefeated()) {
            squad_.setTarget(squad_.getPosition());
            finished_ = true;
        }
        //TODO: add range check so that a squad retreating can be lost
        else if ((sequenceNbr % RETARGET_PERIOD) == Int(0)) { //gives a chance for units to individually look for targets
            squad_.retargetSquad(*target.get());
        }
    }

    return true;
}

bool ChargeSquadOrder::finished()
{
    return finished_;
}

}; //sim
}; //spelprog2
